﻿using System.Diagnostics;
using System.IO.Ports;
using TouchSocket.Core;
using TouchSocket.SerialPorts;
using TouchSocket.Sockets;

namespace Om.Ptz;

public class Ptz
{
    public SerialPortClient Client = new SerialPortClient();

    /// <summary>
    /// 是否正在运动
    /// </summary>
    public bool IsMoving => IsHorizontalMoving || IsVerticalMoving;

    public bool IsHorizontalMoving { get; set; } = false;
    public bool IsVerticalMoving { get; set; } = false;

    private double _horizontalAngle;
    private double _verticalAngle;

    private readonly EnumJogDirection[] _onlyHJogDirections = new[] { EnumJogDirection.Left, EnumJogDirection.Right };
    private readonly EnumJogDirection[] _onlyVJogDirections = new[] { EnumJogDirection.Up, EnumJogDirection.Down };
    /// <summary>
    /// 水平方向速度
    /// </summary>
    private int _speedH = 30;
    /// <summary>
    /// 垂直方向速度
    /// </summary>
    private int _speedV = 30;
    /// <summary>
    /// 云台地址
    /// </summary>
    private readonly int _addr = 0x01;


    private IWaitingClient<ISerialPortClient, IReceiverResult> _spWaitClient = null;
    public async Task Connect(string com)
    {
        if(Client.Online) return;

        Client.Setup(new TouchSocketConfig()
            .SetSerialPortOption(new SerialPortOption()
            {
                PortName = com,
                BaudRate = 9600,
                DataBits = 8,
                Parity = Parity.None,
                StopBits = StopBits.One
            })
            .SetSerialDataHandlingAdapter(() => new PeriodPackageAdapter
                {
                    CacheTimeout = TimeSpan.FromMilliseconds(500),
                })
        );
        var res = await Client.TryConnectAsync();
        if(!res.IsSuccess)
            throw new Exception(res.Message);
        _spWaitClient = Client.CreateWaitingClient(new WaitingOptions()
        {
            FilterFunc = response =>
            {
                return true;
            }
        });
    }

    public async Task Disconnect()
    {
        await Client.SafeCloseAsync();
    }

    /// <summary>
    /// 设置速度
    /// </summary>
    /// <param name="speed">速度值 0-63</param>
    /// <param name="orientation">方向</param>
    /// <returns></returns>
    public Task SetSpeed(EnumOrientation orientation, int speed)
    {
        if (speed <= 0) speed = 0;
        if (speed > 63) speed = 63;
        if (orientation == EnumOrientation.Horizontal)
        {
            _speedH = speed;
        }
        else
        {
            _speedV = speed;
        }
        return Task.CompletedTask;
    }
    /// <summary>
    /// 获取速度
    /// </summary>
    /// <param name="orientation"></param>
    /// <returns></returns>
    public Task<int> ReadSpeed(EnumOrientation orientation)
    {
        return Task.FromResult(orientation == EnumOrientation.Horizontal ? _speedH : _speedV);
    }

    /// <summary>
    /// 读取角度
    /// </summary>
    /// <param name="orientation"></param>
    /// <returns></returns>
    public async Task<double> ReadAngle(EnumOrientation orientation)
    {
        var angle = orientation == EnumOrientation.Horizontal ? _horizontalAngle : _verticalAngle;
        if (angle == 0)
            return await ReadAngleDirectly(orientation);
        return angle;
    }
    /// <summary>
    /// 获取角度（直接读取设备）
    /// </summary>
    /// <param name="orientation"></param>
    /// <returns></returns>
    public async Task<double> ReadAngleDirectly(EnumOrientation orientation)
    {
        try
        {
            var p = orientation == EnumOrientation.Horizontal ? 0x51 : 0x53;
            var cmd = CmdHex(0, p, 0, 0);
            var response = await SendCmdAndReturnResponse(cmd);
            var angle = Helper.ParseAngle(response);
            _ = orientation == EnumOrientation.Horizontal ? _horizontalAngle = angle : _verticalAngle = angle;
            return angle;
        }
        catch (Exception e)
        {
            return 999;
        }
    }

    /// <summary>
    /// 点动
    /// </summary>
    /// <param name="jogDirection">风向</param>
    /// <returns></returns>
    public async Task Jog(EnumJogDirection jogDirection)
    {
        var p3 = 0x00;
        var p4 = (int)jogDirection;
        var p5 = _speedH;
        var p6 = _speedV;
        
        var cmd = CmdHex( p3, p4, p5, p6);
        await Client.SendAsync(cmd);
        IsHorizontalMoving = !_onlyVJogDirections.Contains(jogDirection);
        IsVerticalMoving = !_onlyHJogDirections.Contains(jogDirection);
        ReadAngleWhenJog(jogDirection);
    }

    /// <summary>
    /// 停止
    /// </summary>
    /// <returns></returns>
    public async Task Stop()
    {
        //FF 01 00 00 00 00 01 
        var cmd = CmdHex(0,0,0,0);
        await Client.SendAsync(cmd);
        IsHorizontalMoving = false;
        IsVerticalMoving = false;
        await ReadAngleDirectly(EnumOrientation.Horizontal);
        await ReadAngleDirectly(EnumOrientation.Vertical);
    }

    /// <summary>
    /// 转动到指定角度，瞬时不等待
    /// </summary>
    /// <param name="orientation">方向</param>
    /// <param name="angle">角度。水平（0~360），垂直（-90~90）</param>
    /// <returns></returns>
    public async Task MoveTo(EnumOrientation orientation, double angle)
    {
        var p3 = 0x00;
        var p4 = orientation == EnumOrientation.Horizontal ? 0x4B : 0x4D;
        var data = Helper.AngleToCmdData(orientation, angle);
        var p5 = data.data1;
        var p6 = data.data2;

        var cmd = CmdHex(p3, p4, p5, p6);
        //var cmdStr = CmdStr(p3, p4, p5, p6);
        await Client.SendAsync(cmd);
        _ = orientation == EnumOrientation.Horizontal ? IsHorizontalMoving = true : IsVerticalMoving = true;
        await WaitForStop(orientation);
    }


    private string CmdStr(int p3, int p4, int p5, int p6)
    {
        var p7 = (_addr + p3 + p4 + p5 + p6) & 0xFF;

        var cmd = $"{0xFF:X2} {_addr:X2} {p3:X2} {p4:X2} {p5:X2} {p6:X2} {p7:X2}";
        return cmd;
    }
    private byte[] CmdHex(int p3, int p4, int p5, int p6)
    {
        // 计算校验和，取低 8 位
        byte p7 = (byte)((_addr + p3 + p4 + p5 + p6) & 0xFF);

        byte[] result = new byte[]
        {
            0xFF,
            (byte)_addr,
            (byte)p3,
            (byte)p4,
            (byte)p5,
            (byte)p6,
            p7
        };

        return result;
    }

    /// <summary>
    /// 等待停止
    /// </summary>
    /// <param name="orientation"></param>
    /// <returns></returns>
    private async Task WaitForStop(EnumOrientation orientation)
    {
        var lastAngle = await ReadAngle(orientation);
        var isMoving = orientation == EnumOrientation.Horizontal ? IsHorizontalMoving : IsVerticalMoving;
        while (isMoving)
        {
            await Task.Delay(500);
            var currentAngle = await ReadAngleDirectly(orientation);
            if (Math.Abs(currentAngle - lastAngle) < 0.01)
            {
                _ = orientation == EnumOrientation.Horizontal ? IsHorizontalMoving = false : IsVerticalMoving = false;
                break;
            }
            lastAngle = currentAngle;
            isMoving = orientation == EnumOrientation.Horizontal ? IsHorizontalMoving : IsVerticalMoving;
        }
    }

    private async Task ReadAngleWhenJog(EnumJogDirection jogDirection)
    {
        while (IsMoving)
        {
            await Task.Delay(500);
            if (!_onlyHJogDirections.Contains(jogDirection))
            {
                await ReadAngleDirectly(EnumOrientation.Vertical);
            }
            if (!_onlyVJogDirections.Contains(jogDirection))
            {
                await ReadAngleDirectly(EnumOrientation.Horizontal);
            }
        }
    }

    private readonly SemaphoreSlim _semaphoreSlim = new(1);
    private List<byte> _receivedData;
    private async Task<byte[]> SendCmdAndReturnResponse(byte[] cmd, int timeout = 2000)
    {
        await _semaphoreSlim.WaitAsync();
        try
        {
            var res = await _spWaitClient.SendThenReturnAsync(cmd);
            return res;
        }
        catch (Exception e)
        {
            return Array.Empty<byte>();
        }
        finally
        {
            _semaphoreSlim.Release();
        }

    }

}